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Abstract:  The  minimum-time  path  for  a  robot  arm  has  been  a  long¬ 
standing  and  unsolved  problem  of  considerable  interest.  We  present  a  gen¬ 
eral  solution  to  this  problem  that  involves  joint-space  tesselation,  a  dynamic 
time-scaling  algorithm,  and  graph  search.  The  solution  incorporates  full  dy¬ 
namics  of  movement  and  actuator  constraints,  and  can  be  easily  extended 
for  joint  limits  and  workspace  obstacles,  but  is  subject  to  the  particular 
tesselation  scheme  used.  The  results  presented  show  that,  in  general,  the 
optimal  paths  are  not  straight  lines,  but  rather  curves  in  joint-space  that 
utilise  the  dynamics  of  the  arm  and  gravity  to  help  in  moving  the  arm  faster 
to  its  destination.  Implementation  difficulties  due  to  the  tesselation  and  to 
combinatorial  proliferation  of  paths  are  discussed. 
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1  Introduction 

The  determination  of  the  time-optimal  path  for  manipulators  is  an  im¬ 
portant  problem  in  robot  trajectory  planning.  This  paper  presents  a  general 
solution  to  this  problem  involving  joint  space  tesselation  and  graph  search 
that  is  made  relatively  efficient  through  use  of  time-scaling  properties  of 
dynamics.  The  minimum-time  requirement  imposes  constraints  on  the  ve¬ 
locities  at  each  point  on  our  grid,  so  that  tesselation  in  velocity  space  is 
not  needed.  Our  algorithm  takes  into  account  a  full  dynamic  model  of  a 
manipulator  and  actuator  torque  limits  in  arriving  at  the  time-optimal  tra¬ 
jectory,  i.e.  the  path  and  the  time  dependence  along  the  path,  from  any 
given  starting  state  to  any  final  state.  Kinematic  constraints  due  to  joint 
limits  and  the  presence  of  obstacles  are  quite  easily  incorporated  into  the 
solution. 

Attempts  in  the  past  at  solving  this  problem  have  fallen  short  of  a  gen¬ 
eral  solution.  Most  commonly,  researchers  have  linearized  the  dynamics 
in  order  to  apply  standard  techniques  of  linear  optimal  control  theory  to 
the  solution.  The  earliest  attempt  along  these  lines  appeared  in  (Kahn, 
1969;  Kahn  and  Roth,  1971),  where  the  expected  bang-bang  solution  with 
multiple  switching  points  was  derived.  Approaches  based  on  dynamics  lin¬ 
earization  have  recently  been  cast  into  doubt  due  to  time-scaling  proper¬ 
ties  of  dynamics  (Hollerbach,  1983a,  1983b,  1984),  since  it  can  be  shown 
that  the  velocity  product  terms  have  the  same  significance  relative  to  the 
acceleration  dynamic  terms  for  all  speeds  of  movement.  Thus  the  main 
presumption  used  to  justify  linearization,  namely  that  the  velocity  product 
terms  can  be  ignored  because  they  are  only  significant  at  higher  movement 
speeds,  is  fundamentally  wrong. 

A  second  class  of  solutions  do  take  into  account  the  full  dynamics  but 
presume  a  bang-coast-bang  form  of  control,  again  by  analogy  to  the  lin¬ 
ear  optimal  case.  Scheinman  and  Roth  (1984)  assume  a  bang- coast- bang 
control  form  for  each  joint  where  (1)  each  control  has  a  maximum  of  two 
switching  points  and  (2)  the  total  positive  torque  time  is  equal  to  the  total 
negative  torque  time.  Their  solution  is  an  iterative  one,  where  they  set  a 
time,  solve  for  the  distances  covered  and  the  velocities  attained,  and  adjust 
the  switching  times  until  a  satisfactory  solution  is  found.  Our  results  indi- 
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cate  that  these  presumptions  about  control  are  in  general  not  valid  for  the 
true  time-optimal  solution. 

Brown  (1984)  independently  developed  an  approach  based  on  a  afute 
spare  tessclation  and  graph  search,  which  nevertheless  differs  from  our  ap¬ 
proach  in  critical  ways.  Brown  presumes  a  bang-coast-bang  solution  with 
fixed  switching  points;  all  possible  combinations  of  bangs  and  coasts  yield 
9  distinct  torque  patterns.  At  a  given  point  in  state  space  all  9  torque 
patterns  are  applied  for  a  set  period  of  time,  and  the  dynamics  are  inte¬ 
grated  to  find  the  resultant  state  space  point.  The  resultant  state  sp;*ce 
point  is  rounded  to  the  nearest  neighbour  in  the  tesselation.  The  network 
is  searched  via  standard  procedures  to  find  the  fastest  trajectory.  Again, 
the  presumption  on  control  makes  his  solutions  non-optimal. 

Purely  kinematic  approaches  were  used  by  Luh  and  Walker  (1977),  Luh 
and  Lin  (1981),  and  Lin,  Chang  and  Luh  (1983),  who  attempt  to  find  the 
sequence  of  time  intervals  that  minimize  the  total  time  spent  on  moving 
|gj)  t  between  two  points  in  space.  The  three  approaches  differ  in  the  details 

of  the  algorithms,  but  they  all  suffer  from  the  same  shortcomings:  the 
dynamics  of  the  arm  are  not  considered  at  all,  so  that  the  constraints 
consist  of  sets  of  bounds  on  position,  velocity,  and  acceleration.  These 
bounds  are  imposed  by  the  weakest  configuration  of  the  arm,  so  that  the 
motion  in  other  areas  of  the  workspace  is  sub-optimal.  In  addition,  all  three 
algorithms  require  as  input  a  set  of  knot  points,  which  define  a  path.  This 
path  is  not  necessarily  the  best  possible  one. 

The  new  development  that  facilitates  the  approach  presented  in  this  pa¬ 
per  is  the  discovery  of  a  fundamental  time-scaling  property  of  manipulator 
dynamics.  This  property  was  utilized  to  solve  for  the  time-optimal  trajec¬ 
tory  along  a  predetermined  path  by  (Bobrow,  1982;  Bobrow,  Dubowsky, 
and  Gibson,  1983;  Dubowsky  and  Shiller,  1984).  The  equations  of  motion 
are  written  in  terms  of  the  distance  along  the  Cartesian  path  taken  by 
the  arm,  and  the  bounds  on  the  torques/forces  obtainable  from  the  motors 
are  used  to  construct  a  curve  in  distance-velocity  space  that  constitutes 
an  upper  limit  on  the  performance  of  the  arm.  A  set  of  switching  points 
is  then  found  that  move  the  arm  as  close  as  possible  to  the  limit  curve. 
In  his  Ph.D.  thesis,  Bobrow  proves  the  optimality  of  this  algorithm.  One 
actuator  is  always  saturated,  and  the  others  adjust  their  torques  so  that 


some  constraints  on  the  motion  are  not  violated.  The  only  problem  with  it 
is  that  the  path  has  to  be  known  beforehand.  A  very  similar  approach  was 
developed  later  by  Shin  and  McKay  (1983),  with  the  only  difference  being 
a  parametrization  of  a  path  in  joint  space  instead  of  in  Cartesian  space. 

Hollerbach  (1983a,  1983b,  1984)  independently  developed  the  time¬ 
scaling  property  of  dynamics  in  joint  space  and  used  a  simplified  form 
of  this  property  to  scale  a  fixed  velocity  profile  for  maximum  speed  given 
actuator  constraints.  This  general  formulation  has  been  discretized  for  our 
optimization  algorithm. 

The  next  section  introduces  the  problem  in  a  more  rigorous  fashion,  and 
describes  the  way  with  which  we  manage  to  work  in  state  space ,  yet  tesselate 
only  in  joint  space.  Section  3  introduces  the  Dynamic  Scaling  algorithm, 
which  is  used  to  calculate  the  times  of  travel  along  each  segment  of  the  grid, 
along  with  the  required  torques  and  the  possible  velocities  at  the  end  of  the 
segment,  given  initial  conditions  on  position  and  velocity  at  the  beginning 
of  the  segment.  Section  4  presents  results  of  a  2-D  simulation,  and  section 
5  concludes  with  a  discussion. 

2  The  General  Solution 

The  objective  is  to  move  the  arm  from  point  A  to  point  B  in  state  space 
in  minimum  time.  Given  are: 

•  the  kinematic  and  inertial  parameters  of  the  manipulator, 

•  the  dynamic  equations 

r  =  H  m)O(t)  +  #(t)rC(#(f))#(t)  +  g  (*(«)).  (1) 

where  r  is  the  vector  of  joint  torques,  $  is  the  n-dimensional  joint- 
space  position  vector,  H  is  a  n  x  n  symmetric  inertia  matrix,  C  is 
a  n  x  n  x  n  tensor,  n  is  the  number  of  joints,  and  g  is  a  gravity 
dependent  vector,  and 

•  constant  bounds  on  the  torques/forces  available  from  the  motors, 


which  can  be  made  state  dependent  without  modifying  the  algorithm. 

The  main  idea  behind  our  algorithm  is  to  tesselate  joint  apace  into  a 
grid.  The  requirement  for  time-optimality,  together  with  the  dynamics  of 
the  arm,  constrain  the  number  of  velocities  possible  at  each  position  node, 
given  the  position  and  velocity  at  the  previous  node,  so  that  there  is  no 
need  to  tesselate  velocity  space.  A  tree  of  all  possible  state- space  paths  can 
be  constructed,  and  then  searched  for  the  minimum-time  path. 

In  terms  of  assigning  velocity  tesselations  to  each  position  node,  the 
results  of  (Bobrow  1982)  show  that  one  actuator  is  always  at  a  torque 
bound.  Therefore,  there  are  at  most  2n  possible  transitions  from  a  given 
state  to  a  neighbouring  position.  The  time  scaling  algorithm  presented  in 
the  next  section  determines  for  each  transition  the  velocity  at  this  neigh¬ 
bouring  position  and  the  transition  time.  Many  of  these  2n  transitions  can 
be  discarded  immediately,  due  either  to  saturation  in  one  of  the  actuators 
causing  the  torque  in  one  of  the  other  motors  to  exceed  its  bound,  or  to  a 
contradictory  situation  where  the  velocity  is  in  the  direction  opposite  to  the 
position  increment.  In  our  2-D  implementation,  the  number  of  solutions  at 
each  state  space  point  was  usually  two  or  less. 

2.1  Graph  Search 

The  graph  search  can  be  made  considerably  more  efficient  through  the 
use  of  some  general  search  techniques:  (1)  best  first  search,  which  chooses 
to  expand  the  best  path  so  far,  and  (2)  branch  and  bound,  which  uses 
an  upper  bound  on  the  travel  time  to  eliminate  any  path  with  a  partial 
cost  larger  than  the  upper  bound.  The  choice  here  was  to  first  run  the 
algorithm  with  a  very  sparse  grid,  and  use  the  time  found,  rounded  up  in 
some  suitable  manner  which  depends  on  the  magnitude  of  the  result,  as  the 
upper  bound  for  a  second  run  with  a  finer  grid. 

The  procedure  for  finding  the  optimal  path  follows. 

1.  Form  a  queue  of  paths,  containing  an  empty  path  (one  with  only  the 
starting  node). 

2.  Until  goal  is  reached  (success),  or  the  queue  is  empty  (failure): 
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(a)  Remove  the  first  path  from  the  queue. 

(b)  Calculate  the  positions  of  the  next  points  reachable  from  the 
current  (last  in  path)  point. 

(c)  For  each  one  of  the  next  points,  the  dynamic  scaling  algorithm, 
as  will  be  described  in  the  next  section,  is  used  to  find  the  pos¬ 
sible  velocities  at  the  next  points,  and  the  times  of  travel. 

(d)  New  paths  are  formed  from  the  old  path  and  from  all  admissi¬ 
ble  new  points  (a  point  is  inadmissible  if  one  of  the  calculated 
torques  exceeds  its  bound,  if  the  velocity  of  one  joint  is  in  the 
opposite  direction  to  its  position  increment,  or  the  current  total 
time  of  travel  exceeds  the  upper  bound.) 

(e)  Add  new  paths  to  the  queue  and  sort  it  such  that  the  path  with 
the  smallest  cost  is  on  top. 

3  The  Dynamic  Scaling  Algorithm 

The  previous  section  described  the  global  solution  to  finding  the  optimal 
path.  This  section  describes  the  local  solution  for  the  minimum  traveling 
time  between  nodes,  the  achieved  velocity  for  each  permissible  transition, 
and  the  corresponding  torques.  We  rely  on  a  reformulation  of  the  dynam¬ 
ics  of  a  manipulator,  which  indicates  how  the  underlying  dynamics  change 
when  the  time  dimension  of  a  trajectory  changes  (Hollerbach,  1984).  Sup¬ 
pose  the  time  dependence  along  a  fixed  path  is  changed  from  0(f)  to  0(r(t)), 
where  the  time  scaling  factor  r(f)  is  a  strictly  increasing  function  of  time 
with  r(0)  =  0  and  r(fj)  =  f;  for  some  fi  >  0.  Then  the  joint  torques  r'(f) 
for  the  time-scaled  movement  are  related  to  the  original  movement  torques 
r(t)  by: 

r'(t)  =  r’[r(r)  -  g(*(r))J  +  f  H(#(r))  •  ^  +  *(»(!))  (3) 

This  genera]  formulation  is  now  discretized,  with  known  position  0(»  — 1) 
and  0(t),  respectively,  at  the  beginning  and  end  of  the  segment,  and  known 
velocity  0(i  —  1)  at  the  beginning  of  the  segment.  Instead  of  looking  at  the 


beginning  of  the  segment  (i.e.  point  t  —  1),  consider  the  mid-point.  Define 
an  average  velocity  and  acceleration 


where  the  distance  A0(i)  =  0(i)  -  0(i  -  1)  is  covered  in  time  At.  The 
important  point  here  is  that  no  functional  relationship  is  assumed  between 
0(i  -  and  0(i  -  ^),  but  rather  that  these  relationships  are  used  as  pre¬ 
dictors  that  could  be  subject  to  correction.  By  assuming  a  linear  velocity 
(constant  acceleration)  we  can  find  the  velocity  at  the  end  of  the  segment: 

Hi)  =  -  Hi  -  1)  (6) 

Substituting  this  into  our  definition  of  the  average  acceleration,  we  obtain: 


(7) 

Suppose  the  time  of  travel  was  As  instead  of  At.  The  new  average 
velocity  and  acceleration  at  the  mid-point  are  related  to  the  old  by 

(8) 


We  call  these  quantities  “scaled”.  If  the  scaled  velocity  and  acceleration 
are  substituted  into  the  equations  of  motion  (1),  an  equation  for  t',  the 
torque  required  to  cover  the  distance  in  As,  in  terms  of  r,  the  torque  that 
was  used  when  the  distance  was  covered  in  At,  is  obtained: 


r'  =  f£(r~g)  +  2^£^H'®(,‘l)+g  (10) 

Rewriting  equation  (10)  as  a  quadratic  equation  for  As: 

As2(r'  -  g)  +  2AsH  •  0{i  -  1)  -  [Af2(r  -  g)  +  2AfH  •  0{i  -  1)]  =  0  (11) 
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These  are  n  equations  for  the  unknown  As,  one  for  each  joint,  which  are 
found  by  substituting  the  torque  bounds  for  r'.  They  all  have  to  be  satisfied 
simultaneously  in  order  to  achieve  optimality  without  violating  one  of  the 
constraints.  This  development  leads  to  a  simple  procedure  to  calculate  the 
minimal  time  to  travel  from  point  i  -  1  to  point  t,  and  the  combination  of 
the  torques  that  will  do  it: 

1.  Calculate  6{i  -  1/2),  6{i  -  1/2),  and  0(i  -  1/2),  using  the  known 
0(i  —  1),  A 0(i),  and  0[i  —  1),  and  an  arbitrarily  chosen  A t  (setting 
At  =  1  is  the  most  efficient). 

2.  Solve  the  equations  of  motion  (l)  for  the  corresponding  r. 

3.  For  each  joint,  use  equation  (11)  to  find  As,  where  r'  is  the  vector  of 
given  torque  bounds.  Do  it  for  every  one  of  the  bounds. 

4.  Use  all  the  As  found  in  step  3,  together  with  the  corresponding  torque 
bound,  to  calculate  the  torques  for  the  other  joints  from  equation  (10). 
Retain  only  those  time/torque  combinations  that  do  not  exceed  the 
bounds. 

5.  The  permited  velocities  at  the  end  of  the  segment  can  be  calculated 
from  equation  (6),  with  A t  replaced  by  As. 

4  Implementation 

The  minimum-time  algorithm  is  illustrated  by  simulation  with  a  two 
degree  of  freedom,  planar  manipulator  (Horn,  1975;  Brady,  Hollcrbach, 
Johnson,  Lozano-Perez,  and  Mason,  1982,  chapter  1)  on  a  Symbolics  3600 
Lisp  Machine.  The  equations  of  motion  are 

Tj  =  [/j  Ii  +  “(ro^i  +  rnil\)  +  +  fn2hhc2\^i 

4 

-t-(/2  +  Zrn2^l^2c2)^2  ~  —  vnj/i ^2*2^2  —  rr^2^l^282^l^2  (12) 

+  l2m2*2c^2  +  (  2 TTl1  +  m2)cl)& 


Figure  1:  A  2  degree  of  freedom  planar  arm.  l\  =  h  =  0.5m,  mj  —  50kg, 
m 2  =  30kg,  I\  =  5 kg  •  m2,  I2  =  3A:y  •  m2. 

r2  =  (^2  +  7m2*2  +  Am2^1^2C2)^J  +  (^2  +  — rr»2/f  )^2 

4  L  4 

+  -”12*1*232^1  +  -m2*2e1+2  9 

(13) 

where  9,,  *i,  m,,  and  /,  are  the  joint  angle,  length,  mass,  and  moment  of  in¬ 
ertia,  respectively,  of  link  *,  s,(c,j  are  the  sine(cosine)  of  joint  t  variable,  and 
cit2  is  cos(9!  -(-  02).  The  values  for  the  link  lengths,  masses,  and  moments 
of  inertia  were  taken  from  (Asada,  1984)  (Fig.  1).  The  torque  bounds  were 
chosen  as 

-350A*m  <  rj,  <  350Nm  ,  , 

— 100 JVm  <  T9j  <  lOOfVm  .  [  ' 

4.1  Tesselation  of  joint  space 

The  manner  of  tesselation  is  illustrated  in  Fig.  2  for  this  two- link  manip¬ 
ulator.  The  joint  positions  are  first  tesselated  by  connecting  the  beginning 
and  final  nodes  by  a  straight  line  and  dividing  this  line  into  k  equal  inter¬ 
vals.  The  remainder  of  the  position  grid  is  defined  normal  to  this  line  by 
translating  the  tesselated  points  at  integer  multiples  of  the  point  spacing. 
From  a  given  position  node  three  paths  are  allowed:  one  path  is  parallel  to 


B 

Figure  2:  The  basic  grid  unit 


the  original  straight  line  and  skips  one  grid  point  (from  A  to  C),  the  other 
two  paths  connect  diagonally  relative  to  the  first  path  in  the  direction  of 
the  goal  (from  A  to  B  and  A  to  D).  See  Fig.  3  for  an  example  of  a  3  x  3 
grid. 

The  number  of  paths  leaving  each  node  is  limited  to  3  as  a  compromise 
between  representation  of  path  curvature  and  combinatorial  proliferation 
of  possible  paths.  This  did  create  difficulties  due  to  discontinuities,  which 
were  handled  with  special  measures  described  later.  Given  the  influence  of 
the  choice  of  the  tesselation  number  k  on  the  number  of  paths  and  hence 
on  computation  time,  a  usual  value  of  k  =  15  was  used.  The  largest  value 
of  k  used  was  20.  To  further  reduce  the  search,  the  two  corners  of  the 
rectangular  grid  not  on  the  diagonal  connecting  the  start  and  the  goal  were 
cut  away  as  it  was  noticed  that  the  optimal  paths  generated  by  the  planner 
tended  to  concentrate  around  the  diagonal. 

4.2  The  problem  of  corners 

One  implication  of  tesselating  space  into  a  rectangular  grid  is  that  the 
motion  may  be  discontinuous:  when  moving  from  point  A  to  C  through  B 
(Fig.  2),  the  velocity  has  to  drop  to  zero  at  point  B,  or  change  direction 
instantaneously,  which  implies  infinite  accelerations.  Therefore  the  corner 
has  to  be  smoothed  out  by,  for  example,  fitting  a  circle  between  points  A  and 


C.  Since  our  local  algorithm  is  defined  for  a  straight  line,  the  circf 
have  to  be  approximated  by  a  sequence  of  straight  lines  This  : 
is  impractical  because  of  the  combinatorial  expansion  of  states 
straight  line  sequence  from  start  to  goal,  given  the  possible  new  st.it 
each  application  of  the  time  scaling  algorithm. 

Without  corner  smoothing,  the  velocity  vector  at  the  corner  3 
(Fig.  2),  for  example,  is  not  aligned  with  the  direction  vector  } 
Globally,  the  arm  has  to  decelerate  sharply  after  the  corner  B  10  reac 
C,  and  the  effect  on  the  algorithm  is  to  discard  any  non-straighi-ii; 
as  too  costly.  This  technical  problem  with  our  tesselation  is  unacc 
since  it  is  unlikely  that  a  joint-space  straight-line  path  is  always  op 

A  heuristic  approximation  was  used  of  redefining  the  direction 
the  magnitude  of  the  velocity  vector  at  the  corner  point  B  to  lie  aloi 
For  the  rationale,  assume  that  the  true  path  between  A  and  C  is  ii 
circular  arc  and  that  the  velocity  along  the  arc  is  approximately  co 
Now  let  the  radius  of  the  circle  shrink  gradually,  until  the  circle  bee 
point  at  B.  The  accelerations  required  to  keep  the  arm  on  the  circl 
until  in  the  limit  of  the  point  they  become  infinite.  The  magnitude 
velocity,  however,  does  not  change,  so  that  the  magnitude  of  the  1 
going  into  the  circular  arc  is  the  same  as  the  velocity  magnitude 
out  of  the  other  side  of  the  arc.  In  the  limit,  the  same  is  true  of  po 

4.3  Complexity 

By  counting  the  possible  number  of  paths  on  a  grid,  such  as  tin 
Fig.  3,  the  worst-case  complexity  for  our  planar  example  is  estim 
0(4*),  where  k  is  the  number  of  points  along  one  dimension  of  tl 
This  calculation  assumes  that  only  two  velocities  are  possible  at  th< 
each  segment,  given  some  initial  conditions  at  the  beginning  of  the  si 
This  assumption  is  based  on  observations  during  the  simulation  c 
examples.  It  is  a  conservative  assumption,  inasmuch  as  in  many  si 
only  one  possible  velocity  was  retained,  whereas  there  were  never  m< 
two. 

Another  complexity  factor  is  the  sorting  time  for  the  least  cosl 
in  the  queue.  With  queues  of  thousands  of  paths  the  sorting  opera 


quires  minutes,  which  makes  this  algorithm  too  impractical.  Based  on  this 
experience,  the  sorting  operation  was  eliminated  and  the  new  path  was  in¬ 
serted  in  the  appropriate  place  in  the  already  sorted  queue.  In  our  Lisp 
Machine  implementation  the  improvement  by  simple  merging  was  some¬ 
what  offset  by  creation  of  extraneous  lists,  leading  to  address  space  and 
garbage  collection  problems  that  increased  running  overhead  by  a  half. 

4.4  Results 

The  results  of  two  simulated  runs  of  the  algorithm  are  shown,  with  initial 
and  final  velocities  taken  to  be  zero.  Figures  4-6  show  the  path  for  moving 
from  [—0.5, -1.0]  to  [0.5, 1.0]  in  joint  space,  which  took  0.836  seconds  of 
simulated  time.  The  figures  illustrate  the  tendency  of  the  algorithm  to  find 
a  path  which  is  not  a  straight  line,  neither  in  joint-space  nor  in  Cartesian 
space,  but  is  not  too  far  from  being  one  in  joint  space.  The  best  path 
tends  to  be  one  where  the  arm  rearranges  slightly  its  configuration,  so  that 
dynamically  it  will  be  easier  to  move.  Therefore,  joint  0\  starts  by  moving 
backwards.  Figure  6  shows  the  magnitudes  of  the  acceleration  and  velocity 
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in  Cartesian  space.  Note  the  similarity  to  a  bang-coast-bang  profile,  in 
terms  of  acceleration. 

In  contrast,  moving  from  a  fully  extended  position  ([0.0, 0.0]  in  joint 
space)  along  the  Cartesian  x-axis,  to  the  point  (z,  y\  =  [0.5, 0.0[  (,-71/3, 27r/3; 
in  joint  space),  results  in  a  straight  line,  both  in  Cartesian  space  and  in 
joint  space  (Figures  7-9).  This  is  the  result  of  the  particular  configuration 
of  the  arm,  where  the  elbow  is  pointing  down,  and  gravity  does  not  have 
much  effect  on  the  motion.  Therefore,  the  fastest  path  is  also  the  shortest 
one,  a  straight  line  in  joint  space,  which  for  this  configuration  happens  also 
to  be  a  straight  line  in  Cartesian  space.  This  motion  was  accomplished  in 
0.525  seconds  of  simulated  time. 

From  these,  and  many  more  results  emerges  a  pattern:  usually,  the 
fastest  path  is  close  to  a  straight  line  in  joint  space.  Although  the  grid 
was  not  fine  enough  to  be  able  to  say  so  conclusively,  it  seems  that  the 
path  is  symmetric  about  the  mid-point  between  the  starting  point  and 
the  destination.  The  torques  tend  to  switch  twice  at  the  most,  so  one 
heuristic  used  by  Scheinman  and  Roth  (1984)  seems  to  be  justified  (see  the 
Introduction),  but  contrary  to  their  other  heuristics,  only  one  joint  torque 
is  at  a  maximum  at  a  time,  and  the  total  positive  torque  time  is  not  equal 
to  the  total  negative  torque  time. 

5  Discussion  and  Conclusions 

A  method  for  finding  minimum-time  paths  for  manipulator  arms,  given 
dynamic,  kinematic,  and  geometric  constraints  was  developed.  The  method 
is  based  on  the  creation  of  a  state-space  search  tree  representing  all  possible 
solutions,  and  searching  it  for  the  best  one.  The  search  tree  is  smaller  than 
one  would  initially  expect,  because  velocity  space  was  not  actually  tease- 
lated.  Instead,  it  was  recognized  that  the  requirement  for  time-optimality 
limited  the  number  of  possible  velocities  at  the  end  of  each  segment. 

The  algorithm  as  currently  formulated  and  implemented  is  not  suitable 
for  routine  off-line  trajectory  planning,  as  even  a  15  x  15  grid  required 
minutes  to  hours  of  computation.  We  see  the  current  primary  benefit  as 
providing  the  first  idea  of  what  the  minimal  time  path  looks  like,  however 
long  the  computation  time,  which  was  not  possible  by  any  means  before. 
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From  this  knowledge  it  would  be  possible  to  compare  the  performance  of 
more  efficient  trajectory  planning  methods  and  perhaps  develop  a  method 
that  is  faster  but  sufficiently  close  to  optimal.  It  might  be  possible  to 
improve  the  algorithm  itself  by  new  graph  search  methods  or  hierarchical 
methods  that  zero  in  on  the  optimal  path  while  restricting  the  state  space 
expansion.  The  Lisp  Machine  itself  is  quite  slow  for  numerical  operations, 
and  a  more  algebraically  tuned  system  might  speed  the  algorithm  consid¬ 
erably.  Finally,  it  may  be  possible  to  recast  the  algorithm  into  a  faster 
parallel  computation. 

Several  methods  were  examined  for  pruning  the  search  tree  of  possible 
paths,  but  further  improvements  are  required.  Ideally,  one  would  like  a 
lower  bound  estimate  of  the  cost  from  any  point  in  state  space  to  the  goal 
in  order  to  aid  graph  pruning  (Winston,  1984),  by  a  process  of  comparing 
the  sum  of  this  estimate  and  the  current  cost  at  the  state  space  node  to  the 
upper  bound.  If  a  path  can  be  thrown  out  early,  then  no  time  is  wasted  on 
its  descendents.  We  had  considered  as  a  lower  bound  the  time  to  travel  from 
the  present  node  to  the  goal  while  accelerating  only  and  without  endpoint 
velocity  constraint,  but  we  cannot  prove  this  bound  is  valid.  It  seems  that 
a  true  lower  bound  would  presume  some  form  of  analytic  solution  to  the 
very  problem  we  are  trying  to  solve. 

The  present  tesselation,  which  allows  from  each  node  only  three  direc¬ 
tions  of  motion,  may  exclude  better  paths.  It  would  be  desirable  to  find 
better  tesselation  techniques  that  allow  finer  resolution  of  direction  and 
distance,  yielding  better  approximation  of  curves  by  straight  line  segments, 
yet  curbing  the  combinatorial  proliferation  of  nodes  and  retaining  efficient 
management  of  the  grid.  With  a  finer  direction  tesselation,  the  corner 
problem  would  be  diminished.  Lacking  that,  better  solutions  to  the  corner 
problem  are  required,  since  the  present  approximation  implies  unrealizable 
infinite  accelerations  at  the  corner  point.  Along  the  same  lines,  the  current 
restriction  to  directions  where  at  least  one  joint  moves  closer  to  the  goal 
should  be  relaxed,  since  conceivably  a  solution  might  involve  temporarily 
moving  diametrically  away  from  the  goal. 

The  algorithm  is  easily  extended  to  handle  configuration  constraints  due 
to  joint  excursion  limits  and  to  workspace  obstacles,  when  the  latter  are 
converted  to  obstacles  in  joint  space  (e.g.,  Lozano-Perez,  1982).  Since  these 


translate  into  forbidden  regions  in  joint  space,  the  grid  nodes  within  the 
forbidden  regions  are  excluded  from  the  tesselation.  The  ease  of  incorporal' 
ing  obstacle  avoidance  is  a  general  feature  of  joint  space  tesselation,  as  also 
noted  by  (Brown,  1984),  and  is  an  advantage  over  trajectory  representation 
by  parameterized  paths. 

Finally,  the  algorithm  presented  here  indicates  that  the  fastest  motion 
between  two  points  is  close  to  a  straight  line  in  joint  space.  The  curvature 
at  the  beginning  and  end  of  the  path  in  Fig.  4  may  represent  an  interac¬ 
tion  between  the  shortest  path  in  joint  space  and  the  natural  dynamics  of 
the  movement.  Presently,  it  cannot  be  completely  ruled  out  that  tessela¬ 
tion  problems  contributed  to  some  extent  in  forcing  a  nearly  straight-line 
solution  in  joint  space. 

The  complexity  of  the  state-space  search  may  currently  rule  out  six 
degree  of  freedom  implementations,  but  an  implementation  on  a  three  joint 
manipulator  seems  possible.  Conceivably  a  sparser  grid  might  yield  knot 
points  for  conventional  polynomial  interpolation  that  would  nevertheless 
give  superior  paths.  In  addition,  our  algorithm  can  serve  as  a  starting  point 
for  the  development  of  faster,  less  exact  algorithms,  and  for  the  verification 
of  their  results. 
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Figure  5:  Moving  from  (-0.5,  -1.0)  to  (0.5, 1.0):  velocity,  acceleration,  and 
torque  at  the  joints _  _ 
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Figure  8:  Moving  from  (0.0, 0.0)  to  (-*/3,2*/3):  velocity,  acceleration, 
and  torque  at  the  jointa 
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Figure  9:  Moving  from  (0.0, 0.0)  to  (-it/ 3, 2?r/3):  magnitude  of  tip  velocity 
and  acceleration  in  Cartesian  space 
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